#include "common.h"
#include "tpm.h"

extern int mcg_clk_hz;

volatile struct TPM_MemMap *TPMx[3]=TPM_BASE_PTRS;

int tpm_pwm_init(int tpm, int freq){
    int ps,mod,f;

    if(tpm<0||tpm>2
     ||freq<=0)
        return -1;

    /* Select TPM clock source */
    SIM_SOPT2&=~SIM_SOPT2_TPMSRC_MASK; // Clear TPM clock source settings
    SIM_SOPT2|=SIM_SOPT2_TPMSRC(1); // MCGOUTCLK is selected
    SIM_SOPT2|=SIM_SOPT2_PLLFLLSEL_MASK; // PLL/2 is selected

    /* Enable TPM clock gating */
    switch(tpm){
        case TPM0:
            SIM_SCGC6|=SIM_SCGC6_TPM0_MASK;
            break;
        case TPM1:
            SIM_SCGC6|=SIM_SCGC6_TPM1_MASK;
            break;
        case TPM2:
            SIM_SCGC6|=SIM_SCGC6_TPM2_MASK;
            break;
    }

    /* Initialize TPM clock counter */
    TPM_CNT_REG(TPMx[tpm])=0x00;

    f=mcg_clk_hz/freq/2;

    /* Set prescale factor and modulo value */
    for(ps=0; ps<0x08; ++ps){
        mod=f>>ps;
        if(mod<0x10000)
            break;
    }
    if(!(ps<0x08&&mod<0x10000))
        return -2; // unable to meet the expected frequency

    TPM_MOD_REG(TPMx[tpm])=mod; // Set MOD 
    TPM_SC_REG(TPMx[tpm])=(TPM_SC_PS(ps) // Set Prescale Factor
                          |TPM_SC_CMOD(1)); // Set clock mode

    return 0;
}

int tpm_pwm_duty(int tpm, int channel, int duty){
    int cv;

    if(tpm<0||tpm>2
     ||(tpm==0&&(channel<0||channel>5))||(tpm!=0&&(channel<0||channel>1))
     ||duty<0)
        return -1;

    if((TPM_SC_REG(TPMx[tpm])&TPM_SC_CMOD(1))!=TPM_SC_CMOD(1))
        return -2; // TPM is not in edge-aligned PWM mode

    /* Edge-aligned PWM High-true pulses */
    TPM_CnSC_REG(TPMx[tpm], channel)=TPM_CnSC_MSB_MASK|TPM_CnSC_ELSB_MASK;

    cv=(duty*TPM_MOD_REG(TPMx[tpm])+0x0001)/10000;

    /* Set channel PWM duty(percentage) */
    TPM_CnV_REG(TPMx[tpm], channel)=cv;

    return 0;
}

int tpm_cnt_init(int tpm){
    if(tpm<0||tpm>2)
        return -1;

    /* Select TPM clock source */
    SIM_SOPT2&=~SIM_SOPT2_TPMSRC_MASK; // Clear TPM clock source settings
    SIM_SOPT2|=SIM_SOPT2_TPMSRC(1); // MCGOUTCLK is selected
    SIM_SOPT2|=SIM_SOPT2_PLLFLLSEL_MASK; // PLL/2 is selected
    SIM_SOPT4&=~SIM_SOPT4_TPM0CLKSEL_MASK << tpm; // TPM_CLKIN0 is selected

    /* Enable TPM clock gating */
    switch(tpm){
        case TPM0:
            SIM_SCGC6|=SIM_SCGC6_TPM0_MASK;
            break;
        case TPM1:
            SIM_SCGC6|=SIM_SCGC6_TPM1_MASK;
            break;
        case TPM2:
            SIM_SCGC6|=SIM_SCGC6_TPM2_MASK;
            break;
    }

    /* Reset TPM clock counter */
    TPM_CNT_REG(TPMx[tpm])=0x00;

    /* Set the maximal range for counter */
    TPM_MOD_REG(TPMx[tpm])=TPM_MOD_MOD_MASK;

    /* Set clock mode as counter */
    TPM_SC_REG(TPMx[tpm])=TPM_SC_CMOD(2);

    return 0;
}

uint16 tpm_cnt_read(int tpm){
    uint16 cnt;

    /* Read counter value */
    cnt=TPM_CNT_REG(TPMx[tpm]);

    /* Reset counter */
    TPM_CNT_REG(TPMx[tpm])=0x00;

    return cnt;
}
